#include <math.h>
#include <cmath>
#include <iostream>
#include <fstream>
#include <iostream>
#include <sstream>
#include <omp.h>

#define NUM_PART 200
#define I_X 0
#define I_Y 1
#define I_XVEL 2
#define I_YVEL 3
#define PART 4
#define RADIUS 3
#define DELTAT 0.0002

#define FIRST_CIRCLE 0
#define SECOND_CIRCLE 1

#define GRID_SCALE 1.
int countParticles(double cx, double cy, double r);
void fillParticleCircle(double cx, double cy, double r, double* particles, double startVelocityX, double startVelocityY);
inline void firstSimulationStep(double *in, double *out, const int numParticles);
inline void simulationStep(double *in, double *out, const int numParticles);
inline void zeroOutVelocity(double *in, double *out, const int numParticles);
inline void updatePositions(double *in, double *out, const int numParticles);
inline void influenceParticles(double *in, double *out, const int numParticles);
inline void parallelInfluenceParticles(double *in, double *out, const int numParticles, double deltat);
void ausgabe(double *particles, int length, int timeStamp, int data);
int main()
{
	const int numParticlesFirstCircle = countParticles(0., 0., RADIUS);
	const int numParticlesSecondCircle = countParticles(14., -4.5, RADIUS);
	const int numParticles = numParticlesFirstCircle + numParticlesSecondCircle;
	const int offset2ndCircle = numParticlesFirstCircle*PART;
	
	double* particleBuffer1 = new double[2*numParticles*PART];
	double* particleBuffer2 = &particleBuffer1[numParticles*PART];
	double time=DELTAT/2.;

	fillParticleCircle(0., 0., RADIUS, particleBuffer1, 0, 0);
	fillParticleCircle(14., -4.5, RADIUS, &particleBuffer1[offset2ndCircle], -10, 0);
	memcpy(particleBuffer2, particleBuffer1, numParticles*PART*sizeof(double));
	parallelInfluenceParticles(particleBuffer1, particleBuffer2, numParticles, time);
	int count = 0;
	ausgabe(particleBuffer2, numParticlesFirstCircle, count, FIRST_CIRCLE);
	ausgabe(&particleBuffer2[offset2ndCircle], numParticlesSecondCircle, count, SECOND_CIRCLE);
	int loopUnroll = 2;
	double avg=0.;
	while(time<=4.0001)
	{
		double start = omp_get_wtime();
		parallelInfluenceParticles(particleBuffer1, particleBuffer2, numParticles, DELTAT);time+=DELTAT;
		parallelInfluenceParticles(particleBuffer2, particleBuffer1, numParticles, DELTAT);time+=DELTAT;
		double tick = (omp_get_wtime()-start);
		avg += tick;
		if(count%100 < loopUnroll && count > 0)
		{
			ausgabe(particleBuffer2, numParticlesFirstCircle, count, FIRST_CIRCLE);
			ausgabe(&particleBuffer2[offset2ndCircle], numParticlesSecondCircle, count, SECOND_CIRCLE);
			std::cout << "Step " << count <<" time:" << time << " tick" << tick/loopUnroll << " abs: " << avg << std::endl;
		}
		count+=loopUnroll;
	}
	delete [] particleBuffer1;
	std::cout << "Fertsch! \nBerechnungsdauer: " << avg << "s Durchschnitt pro Iteration: " << avg/(double)count << "s per step";
	std::cin.get();
}

int countParticles(double cx, double cy, double r)
{
	int result=0;
	const double deltaX = 0.4;
	const double rxr = r*r;

	for(double x=-(r+fmod(r,deltaX)) ; x<r+fmod(r,deltaX) ; x+=deltaX)
	{
		for(double y=-(r+fmod(r,deltaX)) ; y<r+fmod(r,deltaX) ; y+=deltaX)
		{
			result += x*x+y*y<rxr?1:0;
		}
	}
	return result;
}

void fillParticleCircle(double cx, double cy, double r, double* particles, double startVelocityX, double startVelocityY)
{
	const double deltaX = 0.4;
	const double rxr = r*r;
	int offset = 0;
	for(double x=-(r+fmod(r,deltaX)) ; x<r+fmod(r,deltaX) ; x+=deltaX)
	{
		for(double y=-(r+fmod(r,deltaX)) ; y<r+fmod(r,deltaX) ; y+=deltaX)
		{
			if(x*x+y*y<rxr)
			{
				particles[offset + I_X] = x+cx;
				particles[offset + I_Y] = y+cy;
				particles[offset + I_XVEL] = startVelocityX;
				particles[offset + I_YVEL] = startVelocityY;
				offset+=PART;
			}
		}
	}
}

void ausgabe(double *particles, int numParticles, int timeStamp, int data)
{
	std::fstream file;
	std::ostringstream oss;
	oss << "PartikelAusgabe" << " " << timeStamp << " " << data << ".txt";
	std::string filename = oss.str();
	file.open(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
	file << "Tropfen1 X\t Tropfen1 Y\tTropfen2 X\t Tropfen2 Y\n";
	int length = numParticles*PART;
	for(int i = 0; i < length; i+=PART)
	{
		file << particles[i+I_X] << "\t" << particles[i+I_Y] << std::endl;
	}
	file.close();
}
#define INFLUENCE \
{\
				xdist = in[jO+I_X]-in[iO+I_X];\
				ydist = in[jO+I_Y]-in[iO+I_Y];\
				dist = 1.e-10+(xdist*xdist+ydist*ydist);\
				dist = ((20.0*dist-8.0)/(dist*dist*dist))*deltat;\
				xvel += dist*xdist;\
				pVelocityField[j*2] -= dist*xdist;\
				yvel += dist*ydist;\
				pVelocityField[j*2+1] -= dist*ydist;\
}
#define LOOP_UNROLLED INFLUENCE jO+=PART; j++;
inline void parallelInfluenceParticles(double *in, double *out, const int numParticles, double deltat)
{
	const int loopUnrolled = 8;
	const int num_threads = 4;
	const int bufferLength = numParticles*PART;
	const double gravity = -9.81;
	double *jParticleVelocities = new double[num_threads*numParticles*2];
	memset(jParticleVelocities, 0, sizeof(double)*num_threads*numParticles*2);
	#pragma omp parallel num_threads(num_threads) shared(in, out, jParticleVelocities)
	{
		double dist;
		double xdist, ydist;
		const int threadId = omp_get_thread_num();
		double *pVelocityField = &jParticleVelocities[threadId*numParticles*2];
		for(int i=threadId ; i<numParticles ; i+=num_threads)
		{
			int iO = i*PART;
			double xvel=in[iO+I_XVEL];
			double yvel=in[iO+I_YVEL];
			int jO = 0;
			int j = 0;
			if(i>0)
			{
				int n = (i+7)/8;
				switch (i%8)
				{
					case 0: do {    LOOP_UNROLLED
					case 7:         LOOP_UNROLLED
					case 6:         LOOP_UNROLLED
					case 5:         LOOP_UNROLLED
					case 4:         LOOP_UNROLLED
					case 3:         LOOP_UNROLLED
					case 2:         LOOP_UNROLLED
					case 1:         LOOP_UNROLLED
					} while (--n>0);
				}
			}
			out[iO+I_XVEL] = xvel;
			out[iO+I_YVEL] = yvel+gravity*DELTAT;
		}
#pragma omp barrier
		int multi = numParticles/num_threads;
		int rest = numParticles%num_threads;
		int start=threadId*multi;
		int end = start+1*multi;
		if(threadId>=num_threads-rest)
		{
			start+=threadId-(num_threads-rest);
			end+=threadId-(num_threads-rest)+1;
		}
		for(int i=start ; i<end ; i++)
		{
			for(int t=0 ; t<num_threads ; ++t)
			{
				out[i*PART+I_XVEL] += jParticleVelocities[t*numParticles*2+i*2];
				out[i*PART+I_YVEL] += jParticleVelocities[t*numParticles*2+i*2+1];
			}
			out[i*PART+I_X] += out[i*PART+I_XVEL]*DELTAT;
			out[i*PART+I_Y] += out[i*PART+I_YVEL]*DELTAT;
		}
	}
	delete [] jParticleVelocities;
}